#include <Eigen/Eigen>
#include <Eigen/Geometry>
#include <Eigen/Core>
#include <vector>
#include <ros/ros.h>
#include <math.h>
#include <vector>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <nlink_parser/LinktrackNodeframe3.h>
#include <nlink_parser/LinktrackNodeframe2.h>
#include <mavros_msgs/OpticalFlowRad.h>
#include <mavros_msgs/Altitude.h>
#include <sensor_msgs/Imu.h>
#include <std_msgs/Float32MultiArray.h>
#include <string>
#include "dwe.h"

using namespace Eigen;
using namespace std;    

// output data
ros::Publisher estimate_pose_pub;
ros::Publisher estimate_vel_pub;
ros::Publisher estimate_viconpose_pub;
ros::Publisher estimate_viconvel_pub;
ros::Publisher estimate_orientation_pub;

//check rel
ros::Publisher estimate_relpose_pub;

// used data
ros::Publisher param_uwb_pub;
ros::Publisher uesd_imu_pub;
ros::Publisher uesd_imuvicon_pub;
ros::Publisher uesd_uwb_pub;
ros::Publisher uesd_uwbvicon_pub;
ros::Publisher estimate_noop_origin_pub;
ros::Publisher estimate_noop_pub;
ros::Publisher uesd_dt_pub;
ros::Publisher camera_A_pub;

// ros::Publisher sensor_uwb_pub;

// ros::Publisher param_uwb_pub;
// ros::Publisher param_uwb_pub;


/**************************** Function Declaration and Definition *******************************/
/**
 * @brief pub estimated pose and vel and IMU_orientation
 */
// estimated xt_real


void PubPoseXt(MatrixXd _estimate_pose ,dwe *filter){
    geometry_msgs::PoseStamped xt_pose_msg;
    xt_pose_msg.header.stamp = ros::Time::now();  
    xt_pose_msg.pose.position.x = _estimate_pose(0,filter->LOOP_num_-1);
    xt_pose_msg.pose.position.y = _estimate_pose(1,filter->LOOP_num_-1);
    xt_pose_msg.pose.position.z = _estimate_pose(2,filter->LOOP_num_-1);
    estimate_pose_pub.publish(xt_pose_msg); 
}


void PubcameraAXt(MatrixXd _estimate_pose ,dwe *filter){
    ROS_INFO("we are in PubcameraAxt");
    geometry_msgs::PoseStamped cameraA_pose_msg;
    // ros::Time time_now;
    // time_now.fromSec(filter->camA_stamp);
    // toSec
    cameraA_pose_msg.header.stamp =  ros::Time::now();
    cameraA_pose_msg.header.frame_id = "map"; // "odom" "base_link" "map
    cameraA_pose_msg.pose.position.x = _estimate_pose(0,filter->LOOP_num_-1);
    cameraA_pose_msg.pose.position.y = _estimate_pose(1,filter->LOOP_num_-1);
    cameraA_pose_msg.pose.position.z = _estimate_pose(2,filter->LOOP_num_-1);
    ROS_INFO("x,y,x:%f %f %f",cameraA_pose_msg.pose.position.x,cameraA_pose_msg.pose.position.y,cameraA_pose_msg.pose.position.z);
    camera_A_pub.publish(cameraA_pose_msg); 
}

// estimated vel_real
void PubVelXt(MatrixXd _estimate_vel ,dwe *filter){
    geometry_msgs::TwistStamped xt_vel_msg;
    xt_vel_msg.header.stamp = ros::Time::now(); 
    xt_vel_msg.twist.linear.x = _estimate_vel(3,filter->LOOP_num_-1);
    xt_vel_msg.twist.linear.y = _estimate_vel(4,filter->LOOP_num_-1);
    xt_vel_msg.twist.linear.z = _estimate_vel(5,filter->LOOP_num_-1);
    estimate_vel_pub.publish(xt_vel_msg); 
} // vel_msg = xt_real_

// relative pose
void PubrelPoseXt(MatrixXd _estimate_relpose ,dwe *filter){
    geometry_msgs::PoseStamped xt_relpose_msg;
    xt_relpose_msg.header.stamp = ros::Time::now();  
    xt_relpose_msg.pose.position.x = _estimate_relpose(0,0);
    xt_relpose_msg.pose.position.y = _estimate_relpose(1,0);
    xt_relpose_msg.pose.position.z = _estimate_relpose(2,0);
    estimate_relpose_pub.publish(xt_relpose_msg); 
}

// relative vicon
void PubviconPoseXt(MatrixXd _viconpose ,dwe *filter){
    geometry_msgs::PoseStamped xt_viconpose_msg;
    xt_viconpose_msg.header.stamp = ros::Time::now();  
    xt_viconpose_msg.pose.position.x = _viconpose(0,0);
    xt_viconpose_msg.pose.position.y = _viconpose(1,0);
    xt_viconpose_msg.pose.position.z = _viconpose(2,0);
    estimate_viconpose_pub.publish(xt_viconpose_msg); 
}

// relative vel
void PubviconVelXt(MatrixXd _viconvel ,dwe *filter){
    geometry_msgs::TwistStamped xt_viconvel_msg;
    xt_viconvel_msg.header.stamp = ros::Time::now();  
    xt_viconvel_msg.twist.linear.x = _viconvel(0,0);
    xt_viconvel_msg.twist.linear.y = _viconvel(1,0);
    xt_viconvel_msg.twist.linear.z = _viconvel(2,0);
    estimate_viconvel_pub.publish(xt_viconvel_msg); 
}


// pub data for px4: IMU_orientation
void PubOrientationXt(MatrixXd _estimate_orientation ,dwe *filter){
    geometry_msgs::PoseStamped xt_orientation_msg;
    xt_orientation_msg.header.stamp = ros::Time::now(); 
    xt_orientation_msg.pose.orientation.w = _estimate_orientation(0,filter->lopt_);
    xt_orientation_msg.pose.orientation.x = _estimate_orientation(1,filter->lopt_);
    xt_orientation_msg.pose.orientation.y = _estimate_orientation(2,filter->lopt_);
    xt_orientation_msg.pose.orientation.z = _estimate_orientation(3,filter->lopt_);
    estimate_orientation_pub.publish(xt_orientation_msg); 
}

// used data : MHE_uwb
void Pubuwb(MatrixXd _used_uwb, dwe *filter) {
    std_msgs::Float32MultiArray uwb_msg;
    uwb_msg.data.resize(filter->uwbnum_); // 设置数组大小为4

    // 填充数据
    for(int i=1; i<=filter->uwbnum_;i++){
        uwb_msg.data[i-1] = _used_uwb(i-1, filter->lopt_);
    }

    // 发布消息
    uesd_uwb_pub.publish(uwb_msg);
}

// sensor data : sensor_uwb
// void Pubsensoruwb(MatrixXd _sensor_uwb, dwe *filter) {
//     std_msgs::Float32MultiArray sensor_uwb_msg;
//     sensor_uwb_msg.data.resize(uwbnum); // 设置数组大小为4

//     // 填充数据
//     for(int i=1; i<=uwbnum;i++){
//         sensor_uwb_msg.data[i-1] = _sensor_uwb(i-1, filter->lopt_);
//     }
//     // 发布消息
//     sensor_uwb_pub.publish(sensor_uwb_msg);
// }

// sensor data : vicon_uwb
void Pubuwbvicon(MatrixXd _vicon_uwb, dwe *filter) {
    std_msgs::Float32MultiArray vicon_uwb_msg;
    vicon_uwb_msg.data.resize(filter->uwbnum_); // 设置数组大小为4

    // 填充数据
    for(int i=1; i<=filter->uwbnum_;i++){
        vicon_uwb_msg.data[i-1] = _vicon_uwb(i-1, filter->lopt_);
    }
    // 发布消息
    uesd_uwbvicon_pub.publish(vicon_uwb_msg);
}

// sensor data : noop_uwb
void Pubnooporigin(MatrixXd _noop_uwb, dwe *filter) {

    geometry_msgs::PoseStamped noop_uwb_msg;
    noop_uwb_msg.header.stamp = ros::Time::now();  
    noop_uwb_msg.pose.position.x = _noop_uwb(0,filter->lopt_);
    noop_uwb_msg.pose.position.y = _noop_uwb(1,filter->lopt_);
    noop_uwb_msg.pose.position.z = _noop_uwb(2,filter->lopt_);

    // 发布消息
    estimate_noop_origin_pub.publish(noop_uwb_msg);
}

// sensor data : noop_uwb filter
void Pubnoop(MatrixXd _noop_uwbfilter, dwe *filter) {

    geometry_msgs::PoseStamped noop_uwbfilter_msg;
    noop_uwbfilter_msg.header.stamp = ros::Time::now();  
    noop_uwbfilter_msg.pose.position.x = _noop_uwbfilter(0,filter->lopt_);
    noop_uwbfilter_msg.pose.position.y = _noop_uwbfilter(1,filter->lopt_);
    noop_uwbfilter_msg.pose.position.z = _noop_uwbfilter(2,filter->lopt_);

    // 发布消息
    estimate_noop_pub.publish(noop_uwbfilter_msg);
}
// void Pubuwb(MatrixXd _used_uwb, dwe *filter) {
//     geometry_msgs::PoseStamped uwb_msg;
//     uwb_msg.header.stamp = ros::Time::now(); 
//     uwb_msg.pose.orientation.w = _used_uwb(0,filter->lopt_);
//     uwb_msg.pose.orientation.x = _used_uwb(1,filter->lopt_);
//     uwb_msg.pose.orientation.y = _used_uwb(2,filter->lopt_);
//     uwb_msg.pose.orientation.z = _used_uwb(3,filter->lopt_);
//     uesd_uwb_pub.publish(uwb_msg); 
// }


// void Pubuwb(MatrixXd _used_uwb ,dwe *filter){
//     geometry_msgs::PoseStamped uwb_msg;
//     uwb_msg.header.stamp = ros::Time::now(); 
//     uwb_msg.pose.position.x = _used_uwb(0,filter->lopt_);
//     uwb_msg.pose.position.y = _used_uwb(1,filter->lopt_);
//     uwb_msg.pose.position.z = _used_uwb(2,filter->lopt_);
//     uwb_msg.pose.position.x = _used_uwb(3,filter->lopt_);
//     uesd_uwb_pub.publish(uwb_msg); 
// }

// used data : MHE_imu
void Pubimu(MatrixXd _used_imu ,dwe *filter){
    geometry_msgs::PoseStamped imu_msg;
    imu_msg.header.stamp = ros::Time::now(); 
    imu_msg.pose.position.x = _used_imu(0,filter->lopt_-1);
    imu_msg.pose.position.y = _used_imu(1,filter->lopt_-1);
    imu_msg.pose.position.z = _used_imu(2,filter->lopt_-1);
    uesd_imu_pub.publish(imu_msg); 
}

void Pubimuvicon(MatrixXd _used_imuvicon ,dwe *filter){
    geometry_msgs::PoseStamped imuvicon_msg;
    imuvicon_msg.header.stamp = ros::Time::now(); 
    imuvicon_msg.pose.position.x = _used_imuvicon(0,filter->lopt_-1);
    imuvicon_msg.pose.position.y = _used_imuvicon(1,filter->lopt_-1);
    imuvicon_msg.pose.position.z = _used_imuvicon(2,filter->lopt_-1);
    uesd_imuvicon_pub.publish(imuvicon_msg); 
}

// used data : MHE_dt
void Pubdt(float _used_dt,float _used_dt_solveonce ,dwe *filter){
    std_msgs::Float32MultiArray dt_msg;
    dt_msg.data.resize(2); // 设置数组大小为2

    // 填充数据
    dt_msg.data[0] = _used_dt;
    dt_msg.data[1] = _used_dt_solveonce;

    uesd_dt_pub.publish(dt_msg); 
}


// void Pubdt(float _used_dt,float _used_dt_solveonce ,dwe *filter){
//     geometry_msgs::PoseStamped dt_msg;
//     dt_msg.header.stamp = ros::Time::now(); 
//     dt_msg.pose.position.x = _used_dt;
//     dt_msg.pose.position.y = _used_dt_solveonce;
//     uesd_dt_pub.publish(dt_msg); 
// }

// param
// void Pubparamuwb(vector<double> _uwb_param, dwe *filter) {
//     std_msgs::Float32MultiArray param_uwb_msg;
//     param_uwb_msg.data.resize(4); // 设置数组大小为4

//     // 填充数据
//     param_uwb_msg.data[0] = _uwb_param[0];
//     param_uwb_msg.data[1] = _uwb_param[1];
//     param_uwb_msg.data[2] = _uwb_param[2];
//     param_uwb_msg.data[3] = _uwb_param[3];

//     // 发布消息
//     param_uwb_pub.publish(param_uwb_msg);
// }

void Pubparamuwb(MatrixXd _uwb_param, dwe *filter) {
    std_msgs::Float32MultiArray param_uwb_msg;
    param_uwb_msg.data.resize(filter->uwbnum_); // 设置数组大小为4

    // 填充数据
    for(int i=1; i<=filter->uwbnum_;i++){
        param_uwb_msg.data[i-1] = _uwb_param(i-1, filter->lopt_);
    }

    // 发布消息
    param_uwb_pub.publish(param_uwb_msg);
}


/*********************************************** Main Function ***********************************************/
int main (int argc, char** argv) 
{
    // ros init
    ros::init(argc, argv, "dwe", ros::init_options::AnonymousName);
    ros::NodeHandle nh;
    ros::Rate loop_rate(25);

    // define dwe_filter and load param
    dwe dwe_filter(nh);
    // load param
    // dwe_filter.loadParameters(nh);

    // notation: we can use a valuable named paramCamera to get the weight of camera parameter


    /// subscriber
    ros::Subscriber imu_new_sub = nh.subscribe<sensor_msgs::Imu>("/IMU_data",1,boost::bind(&dwe::imu_new_cb, &dwe_filter, _1));
    // cb for nodeframe3 mode
    // ros::Subscriber uwb3_sub = nh.subscribe<nlink_parser::LinktrackNodeframe3>("/nlink_linktrack_nodeframe3", 1,  dwe_filter.uwb3_cb);  // revised for 4 uwb cb
    // cb for nodeframe2 mode
    ros::Subscriber uwb2_sub = nh.subscribe<nlink_parser::LinktrackNodeframe2>("/nlink_linktrack_nodeframe2",1,boost::bind(&dwe::uwb2_cb, &dwe_filter, _1));

    ros::Subscriber pos_uav_sub = nh.subscribe<geometry_msgs::PoseStamped>("/csj01/mocap/pos",1,boost::bind(&dwe::pos_uav_cb, &dwe_filter, _1));
    ros::Subscriber vel_uav_sub = nh.subscribe<geometry_msgs::TwistStamped>("/csj01/mocap/vel", 1,boost::bind(&dwe::vel_uav_cb, &dwe_filter, _1));

    ros::Subscriber pos_uwba0_sub = nh.subscribe<geometry_msgs::PoseStamped>("/uwba0/mocap/pos",1,boost::bind(&dwe::pos_uwba0_cb, &dwe_filter, _1));
    ros::Subscriber pos_uwba1_sub = nh.subscribe<geometry_msgs::PoseStamped>("/uwba1/mocap/pos",1,boost::bind(&dwe::pos_uwba1_cb, &dwe_filter, _1));                                    
    ros::Subscriber pos_uwba2_sub = nh.subscribe<geometry_msgs::PoseStamped>("/uwba2/mocap/pos",1,boost::bind(&dwe::pos_uwba2_cb, &dwe_filter, _1));
    ros::Subscriber pos_uwba3_sub = nh.subscribe<geometry_msgs::PoseStamped>("/uwba3/mocap/pos",1,boost::bind(&dwe::pos_uwba3_cb, &dwe_filter, _1));
    ros::Subscriber pos_uwba4_sub = nh.subscribe<geometry_msgs::PoseStamped>("/uwba4/mocap/pos",1,boost::bind(&dwe::pos_uwba4_cb, &dwe_filter, _1));
    ros::Subscriber pos_uwba5_sub = nh.subscribe<geometry_msgs::PoseStamped>("/uwba5/mocap/pos",1,boost::bind(&dwe::pos_uwba5_cb, &dwe_filter, _1));    
    ros::Subscriber pos_uwba6_sub = nh.subscribe<geometry_msgs::PoseStamped>("/uwba6/mocap/pos",1,boost::bind(&dwe::pos_uwba6_cb, &dwe_filter, _1));
    ros::Subscriber pos_uwba7_sub = nh.subscribe<geometry_msgs::PoseStamped>("/uwba7/mocap/pos",1,boost::bind(&dwe::pos_uwba7_cb, &dwe_filter, _1));

    // get the data from camera
    ros::Subscriber camA_sub = nh.subscribe<geometry_msgs::TransformStamped>("/multi_cam_cooperation_ros/T_base_to_coopestimation_from_camA", 1, dwe_filter.camA_cb);
    ros::Subscriber camB_sub = nh.subscribe<geometry_msgs::TransformStamped>("/multi_cam_cooperation_ros/T_base_to_coopestimation_from_camB", 1, dwe_filter.camB_cb);
    ros::Subscriber camC_sub = nh.subscribe<geometry_msgs::TransformStamped>("/multi_cam_cooperation_ros/T_base_to_coopestimation_from_camC", 1, dwe_filter.camC_cb);
    ros::Subscriber camD_sub = nh.subscribe<geometry_msgs::TransformStamped>("/multi_cam_cooperation_ros/T_base_to_coopestimation_from_camD", 1, dwe_filter.camD_cb);
    ros::Subscriber camE_sub = nh.subscribe<geometry_msgs::TransformStamped>("/multi_cam_cooperation_ros/T_base_to_coopestimation_from_camA", 1, dwe_filter.camE_cb);
    ros::Subscriber camF_sub = nh.subscribe<geometry_msgs::TransformStamped>("/multi_cam_cooperation_ros/T_base_to_coopestimation_from_camB", 1, dwe_filter.camF_cb);
    ros::Subscriber camG_sub = nh.subscribe<geometry_msgs::TransformStamped>("/multi_cam_cooperation_ros/T_base_to_coopestimation_from_camC", 1, dwe_filter.camG_cb);
    ros::Subscriber camH_sub = nh.subscribe<geometry_msgs::TransformStamped>("/multi_cam_cooperation_ros/T_base_to_coopestimation_from_camD", 1, dwe_filter.camH_cb);




    /*****************for rosbag check exp *****************/
    // pub : check -- use for rosbag pla check
    estimate_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/rosbag/xt_real/pose", 1);
    estimate_vel_pub = nh.advertise<geometry_msgs::TwistStamped>("/rosbag/xt_real/vel", 1);

    camera_A_pub = nh.advertise<geometry_msgs::PoseStamped>("/rosbag/cameraA/pose",1);



    estimate_viconpose_pub = nh.advertise<geometry_msgs::PoseStamped>("/rosbag/viconxt_real/pose", 1);
    estimate_viconvel_pub = nh.advertise<geometry_msgs::TwistStamped>("/rosbag/viconxt_real/vel", 1);
    
    estimate_relpose_pub = nh.advertise<geometry_msgs::PoseStamped>("/rosbag/viconxt/pose", 1);
    
    estimate_noop_origin_pub = nh.advertise<geometry_msgs::PoseStamped>("/rosbag/noop_origin/pose", 1);      // with filter
    estimate_noop_pub = nh.advertise<geometry_msgs::PoseStamped>("/rosbag/noop/pose", 1);      // with filter

    // estimate_orientation_pub = nh.advertise<geometry_msgs::PoseStamped>("/rosbag/xt/orientation", 1);
    // pub : used data
    uesd_imu_pub = nh.advertise<geometry_msgs::PoseStamped>("/rosbag/used/imu", 1);       // with ori trans
    uesd_imuvicon_pub = nh.advertise<geometry_msgs::PoseStamped>("/rosbag/used/imuvicon", 1);       // with ori trans
    uesd_uwb_pub = nh.advertise<std_msgs::Float32MultiArray>("/rosbag/used/uwb", 1);      // with filter
    uesd_uwbvicon_pub = nh.advertise<std_msgs::Float32MultiArray>("/rosbag/used/uwbvicon", 1);      // with filter
    uesd_dt_pub = nh.advertise<std_msgs::Float32MultiArray>("/rosbag/used/dt", 1);        // loop_dt & solveonce_dt

    // pub : just for data state check
    // sensor_uwb_pub = nh.advertise<std_msgs::Float32MultiArray>("/rosbag/sensor/uwb", 1);  // without filter
    param_uwb_pub = nh.advertise<std_msgs::Float32MultiArray>("/rosbag/param/uwb", 1);    // check the star of uwb1-4
    /*****************for rosbag check exp *****************/

    /*********************************************** Main Loop ***********************************************/
    while(ros::ok()){

        ros::spinOnce();

        // ensure imu msg is obtained
        if (!(dwe_filter.flag_init_imu_)){
            continue;
        }

        dwe_filter.get_data();

        // LOOP_num: current timestep
        ROS_INFO("----------ENTER ROS LOOP----------");
        ROS_INFO("LOOP: %d", dwe_filter.LOOP_num_);

        // for quick ini with wivon
        // for (int i = 1; i <= uwbnum; i++) {
        //     ROS_WARN("DATA ini_pos: %f  %f  %f",dwe_filter.VICON_PosUWB_ini(0,i-1),dwe_filter.VICON_PosUWB_ini(1,i-1),dwe_filter.VICON_PosUWB_ini(2,i-1));
        // }

        // show the data from uav
        ROS_INFO("DATA MHE_imu_: ux:%f uy:%f uz:%f",dwe_filter.MHE_imu_(0,dwe_filter.lopt_), dwe_filter.MHE_imu_(1,dwe_filter.lopt_),dwe_filter.MHE_imu_(2,dwe_filter.lopt_));
        // ROS_INFO("DATA MHE_uwb_: uwb01:%f uwb02:%f uwb03:%f uwb04:%f ",dwe_filter.MHE_uwb_(0,dwe_filter.lopt_),dwe_filter.MHE_uwb_(1,dwe_filter.lopt_),dwe_filter.MHE_uwb_(2,dwe_filter.lopt_),dwe_filter.MHE_uwb_(3,dwe_filter.lopt_));
        ROS_INFO("DATA MHE_uwb_: uwb01:%f uwb02:%f uwb03:%f uwb04:%f uwb05:%f uwb06:%f uwb07:%f uwb08:%f",dwe_filter.MHE_uwb_(0,dwe_filter.lopt_),dwe_filter.MHE_uwb_(1,dwe_filter.lopt_),dwe_filter.MHE_uwb_(2,dwe_filter.lopt_),dwe_filter.MHE_uwb_(3,dwe_filter.lopt_),dwe_filter.MHE_uwb_(4,dwe_filter.lopt_),dwe_filter.MHE_uwb_(5,dwe_filter.lopt_),dwe_filter.MHE_uwb_(6,dwe_filter.lopt_),dwe_filter.MHE_uwb_(7,dwe_filter.lopt_));
        // get initial data xt_ fixed wide
        // if(!dwe_filter.flag_init_ok_){
        //     dwe_filter.LOOP_num_ = dwe_filter.LOOP_num_+1;
        //     dwe_filter.flag_init_imu_ = false; 
        //     loop_rate.sleep();
        //     continue;
        // }

        // get initial data xt_ until takeoff
        if(!dwe_filter.flag_init_ok_){
            // ROS_WARN("----------1----------");
            // pub : output
            PubPoseXt(dwe_filter.xt_real_,&dwe_filter);
            PubVelXt(dwe_filter.xt_real_,&dwe_filter);
            PubviconPoseXt(dwe_filter.VICON_PosUAV_,&dwe_filter);
            PubviconVelXt(dwe_filter.VICON_VelUAV_,&dwe_filter);


            PubcameraAXt(dwe_filter.xt_camera,&dwe_filter);
           //  PubOrientationXt(dwe_filter.MHE_imu_att_,&dwe_filter);

            // relative data
            PubrelPoseXt(dwe_filter.VICON_PosUAV2UWB01_,&dwe_filter);

            // pub : MHE data
            Pubuwb(dwe_filter.MHE_uwb_,&dwe_filter);
            Pubuwbvicon(dwe_filter.VICON_uwb_,&dwe_filter);
            Pubnooporigin(dwe_filter.MHE_uwb_posorigin_,&dwe_filter);
            Pubnoop(dwe_filter.MHE_uwb_pos_,&dwe_filter);
            Pubimu(dwe_filter.MHE_imu_,&dwe_filter);
            Pubdt(dwe_filter.dt_,0,&dwe_filter);
            Pubimuvicon(dwe_filter.VICON_ImuUAV_,&dwe_filter);

            // Pubsensoruwb(dwe_filter.MHE_uwb_sensor_,&dwe_filter);
            Pubparamuwb(dwe_filter.paramUWB_,&dwe_filter);

            dwe_filter.LOOP_num_ = dwe_filter.LOOP_num_+1;
            dwe_filter.flag_init_imu_ = false; 
            loop_rate.sleep();
            continue;
        }
        
        // estimated initial value
        double time_start = ros::Time::now().toSec();  
        dwe_filter.solveonce();  
        double time_end = ros::Time::now().toSec();
        ROS_INFO("consumptiom dt: %.4lf seconds", time_end - time_start);

        // show the Initialized data x1_ and the Estimated data xt_
        // ROS_INFO("Initialized data: x:%f y:%f z:%f",dwe_filter.x1_(0,dwe_filter.lopt_), dwe_filter.x1_(1,dwe_filter.lopt_),dwe_filter.x1_(2,dwe_filter.lopt_));
        ROS_INFO("Estimated   data: x:%f y:%f z:%f",dwe_filter.xt_real_(0,dwe_filter.LOOP_num_-1), dwe_filter.xt_real_(1,dwe_filter.LOOP_num_-1),dwe_filter.xt_real_(2,dwe_filter.LOOP_num_-1));
        ROS_INFO("Error real  data: x:%f y:%f z:%f",(dwe_filter.xt_real_(0,dwe_filter.LOOP_num_-1)-dwe_filter.VICON_PosUAV_(0,dwe_filter.lopt_)), (dwe_filter.xt_real_(1,dwe_filter.LOOP_num_-1)-dwe_filter.VICON_PosUAV_(1,dwe_filter.lopt_)),(dwe_filter.xt_real_(2,dwe_filter.LOOP_num_-1)-dwe_filter.VICON_PosUAV_(2,dwe_filter.lopt_)));
        ROS_INFO("camera estimated data : x:%f y:%f z:%f",dwe_filter.xt_camera(0,dwe_filter.LOOP_num_-1), dwe_filter.xt_camera(1,dwe_filter.LOOP_num_-1),dwe_filter.xt_camera(2,dwe_filter.LOOP_num_-1));        
        // 写入内容到文件
        // dwe_filter.data_writter();

        // pub : output
        PubPoseXt(dwe_filter.xt_real_,&dwe_filter);
        PubVelXt(dwe_filter.xt_real_,&dwe_filter);
        PubviconPoseXt(dwe_filter.VICON_PosUAV_,&dwe_filter);
        PubviconVelXt(dwe_filter.VICON_VelUAV_,&dwe_filter);
        PubcameraAXt(dwe_filter.xt_camera,&dwe_filter);
       //  PubOrientationXt(dwe_filter.MHE_imu_att_,&dwe_filter);

        // relative data
        PubrelPoseXt(dwe_filter.VICON_PosUAV2UWB01_,&dwe_filter);

        // pub : MHE data
        Pubuwb(dwe_filter.MHE_uwb_,&dwe_filter);
        Pubuwbvicon(dwe_filter.VICON_uwb_,&dwe_filter);
        Pubnooporigin(dwe_filter.MHE_uwb_posorigin_,&dwe_filter);
        Pubnoop(dwe_filter.MHE_uwb_pos_,&dwe_filter);
        Pubimu(dwe_filter.MHE_imu_,&dwe_filter);
        Pubdt(dwe_filter.dt_,time_end - time_start,&dwe_filter);
        Pubimuvicon(dwe_filter.VICON_ImuUAV_,&dwe_filter);

        // Pubsensoruwb(dwe_filter.MHE_uwb_sensor_,&dwe_filter);
        Pubparamuwb(dwe_filter.paramUWB_,&dwe_filter);

        // test-
        // if (dwe_filter.LOOP_num_ == 2){
        //     break;
        // }
        // test-

        dwe_filter.LOOP_num_ = dwe_filter.LOOP_num_+1;
        dwe_filter.flag_init_imu_ = false; 
        loop_rate.sleep();
    }
   
    ROS_INFO("----------FINISH ROS LOOP----------");
    return 0;
}
